#include <math.h>
#include "StringSegment.h"
#include "include.h"

StringSegment::StringSegment(Vector2D endPoint, float mass, float restLength, float springConstant)
{
	this->endPoint = endPoint;
	this->mass = mass;
	this->restLength = restLength;
	this->springConstant = springConstant;
	originalSpringConstant = springConstant;
	ResetDampingConstant();
}
StringSegment::~StringSegment()
{
}
void StringSegment::SetSpringConstant(float springConstant)
{
	this->springConstant = springConstant;
	ResetDampingConstant();
}
void StringSegment::ResetDampingConstant()
{
	SetDampingConstant(mass);
}
void StringSegment::SetDampingConstant(float weight)
{
	dampingConstant = 1.5 * sqrt(springConstant * weight);
}
void StringSegment::AddForce(Vector2D force)
{
	totalForce = totalForce + force;
}

void StringSegment::Think(double elapsedTime)
{
	if(mass != 0.0)
	{
		acceleration = totalForce / mass;
	}
	else
	{
		acceleration = Vector2D(0,0);
	}
	velocity = velocity + (acceleration * elapsedTime);
	endPoint = endPoint + (velocity * elapsedTime);
	
	totalForce = Vector2D(0.0, 0.0);
}
void StringSegment::AddGravitationalForce()
{
	AddForce(Vector2D(0.0,GRAVITATIONAL_FORCE) * mass);
}

Vector2D StringSegment::CalculateStringForce(Vector2D * anchor, Object * object, Vector2D * grabPoint)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = *anchor - endPoint;
		direction = !direction;
		Vector2D springForce =  direction * (springConstant * (currentLength - restLength));
		Vector2D velocity = (object->VelocityAtPoint(*grabPoint));
		Vector2D dampingForce = (dampingConstant * velocity);
		return springForce - dampingForce;
	}
	return Vector2D(0.0, 0.0);
}
Vector2D StringSegment::CalculateStringForce(Vector2D * anchor, Vector2D anchorVelocity, Object * object, Vector2D * grabPoint)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = *anchor - endPoint;
		direction = !direction;
		Vector2D springForce =  direction * (springConstant * (currentLength - restLength));
		Vector2D velocity = (object->VelocityAtPoint(*grabPoint));
		Vector2D dampingForce = ((velocity - anchorVelocity) * dampingConstant);
		return springForce - dampingForce;
	}
	return Vector2D(0.0, 0.0);
}
Vector2D StringSegment::CalculateStringForce(Vector2D * anchor)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = *anchor - endPoint;
		direction = !direction;
		Vector2D springForce =  direction * (springConstant * (currentLength - restLength));
		Vector2D dampingForce = (dampingConstant * velocity);
		return springForce - dampingForce;
	}
	return Vector2D(0.0, 0.0);
}
Vector2D StringSegment::CalculateStringForce(Vector2D * anchor, Vector2D anchorVelocity)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = *anchor - endPoint;
		direction = !direction;
		Vector2D springForce =  direction * (springConstant * (currentLength - restLength));
		Vector2D dampingForce = ((velocity - anchorVelocity) * dampingConstant);
		return springForce - dampingForce;
	}
	return Vector2D(0.0, 0.0);
}
Vector2D StringSegment::CalculateGravitationalForce(Vector2D * anchor)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = endPoint - *anchor;
		direction = !direction;
		Vector2D gravitationalForce = Vector2D(0.0,GRAVITATIONAL_FORCE) * mass;
		gravitationalForce = direction * direction.dot(gravitationalForce);
		return gravitationalForce;
	}
	return Vector2D(0.0, 0.0);
}
Vector2D StringSegment::CalculateGravitationalForce(Vector2D * anchor, Object * weight)
{
	currentLength = anchor->dist(endPoint);
	if(currentLength > restLength)
	{
		Vector2D direction = endPoint - *anchor;
		direction = !direction;
		Vector2D gravitationalForce = Vector2D(0.0,GRAVITATIONAL_FORCE) * weight->mass;
		gravitationalForce = direction * direction.dot(gravitationalForce);
		return gravitationalForce;
	}
	return Vector2D(0.0, 0.0);
}
